#!/usr/bin/env python3
# -*- coding: utf-8 -*-
"""
Created on Wed Apr  3 14:10:11 2024

@author: lixiao
"""

import numpy as np
from perpectiveGeometry import (
    SE3_to_se3,
    se3_to_SE3
  )

def unvectorizeLieParameters(paramVec, K, M, N):
  """
  Takes in a parameter vector ((6*M + 3*N) x 1) and 'unvectorizes' it, i.e.,
  converts the vector to a set of projection matrices and 3D points.
  
  Args:
  - paramVec: parameter vector ((6*M + 3*N) x 1)
  - K: camera calibration matrix
  - M: number of views
  - N: number of 3D points
  
  Returns:
  - Ps: projection matrices (M x 3 x 4)
  - X: 3D points in homogeneous coordinates (4 x N)
  """
  # Projection matrices
  Ps = np.zeros((M, 3, 4))
  for i in range(M):
    tempVar = se3_to_SE3(paramVec[6*i:6*(i+1)])
    Ps[i, :, :] = np.dot(K, tempVar[:3, :])
  # 3D points
  # X = np.vstack((np.reshape(paramVec[6*M:], (3, N)), np.ones((1, N))))
  X = np.hstack((np.reshape(paramVec[6*M:], (N, 3)), np.ones((N, 1))))
  return Ps, X.T

def vectorizeLieParameters(para_views, para_points):
  M = para_views.shape[0]
  N = para_points.shape[0]
  para_vec = np.zeros(6*M + 3*N)
  for i in range(M):
    para_vec[i*6:(i+1)*6] = para_views[i]
  
  para_vec[6*M:] = para_points.reshape(-1)

  
  return para_vec






































































